#include <chrono>
#include <cinttypes>
#include <functional>
#include <future>
#include <memory>
#include <string>

#include "common/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class MiniActionClient : public rclcpp::Node
{
public:
    using Fibonacci = common::action::Fibonacci;
    using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

    explicit MiniActionClient(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
        : Node("mini_action_client", options), goal_done(false)
    {
        this->client_ptr = rclcpp_action::create_client<Fibonacci>(
            this->get_node_base_interface(),
            this->get_node_graph_interface(),
            this->get_node_logging_interface(),
            this->get_node_waitables_interface(),
            "fibonacci");
        this->timer = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&MiniActionClient::send_goal, this));
    }

    bool is_goal_done() const
    {
        return this->goal_done;
    }

    void send_goal()
    {
        using namespace std::placeholders;
        this->timer->cancel();
        this->goal_done = false;
        if (!this->client_ptr)
        {
            RCLCPP_ERROR(this->get_logger(), "action client not init");
        }
        if (!this->client_ptr->wait_for_action_server(std::chrono::seconds(10)))
        {
            RCLCPP_ERROR(this->get_logger(), "Action server not available");
            this->goal_done = true;
            return;
        }

        auto goal_msg = Fibonacci::Goal();
        goal_msg.order = 10;
        RCLCPP_INFO(this->get_logger(), "Sending goal");

        auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
        send_goal_options.goal_response_callback =
            std::bind(&MiniActionClient::goal_response_callback, this, _1);
        send_goal_options.feedback_callback =
            std::bind(&MiniActionClient::feedback_callback, this, _1, _2);
        send_goal_options.result_callback =
            std::bind(&MiniActionClient::result_callback, this, _1);
        auto goal_handle_future = this->client_ptr->async_send_goal(goal_msg, send_goal_options);
    }

private:
    rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr;
    rclcpp::TimerBase::SharedPtr timer;
    bool goal_done;

    void goal_response_callback(GoalHandleFibonacci::SharedPtr goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "goal accepted by server, waiting for result");
        }
    }

    void feedback_callback(GoalHandleFibonacci::SharedPtr,
                           const std::shared_ptr<const Fibonacci::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Next number in sequence received: %d", feedback->partial_sequence.back());
    }

    void result_callback(const GoalHandleFibonacci::WrappedResult &result)
    {
        this->goal_done = true;
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
            return;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
            return;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            return;
        }
        RCLCPP_INFO(this->get_logger(), "Result received");
        for (auto number : result.result->sequence)
        {
            RCLCPP_INFO(this->get_logger(), "Result: %d", number);
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto action_client = std::make_shared<MiniActionClient>();
    while (!action_client->is_goal_done())
    {
        rclcpp::spin_some(action_client);
    }
    rclcpp::shutdown();
    return 0;
}